
#include <BStyle>
#include <BPainter>
#include <BRender>
#include <BViewer>
#include <BScene>
#include <BCone>
#include <BCube>
#include <BImage>
#include <BMaterial>
#include <BReadWrite>

#include "SixdofHandler.h"

SixdofHandler::SixdofHandler()
{
	_nodeA = new BBody("nodeA");
	{
		BCube* cube = new BCube(0.2, 12);
		cube->setColor(100, 200, 255);
		_nodeA->insertShape(cube);
		_nodeA->setMass(0);
		_nodeA->move(0, 0, 1);
	}

	_nodeB = new BBody("nodeB");
	{
		BCone* cone = new BCone(0.05, 0.2, 24, 10);
		cone->setColor(200, 100, 0);
		_nodeB->insertShape(cone);
		_nodeB->setMass(1);
	}

	BVector pivotA = _nodeA->space().max() + BVector(3, 0, 0);
	BVector pivotB(0, 0, _nodeB->space().max().z());
	_restraint = new BSixdofRestraint(_nodeA, pivotA, _nodeB, pivotB);
	_restraint->setLinearLowerLimit(BVector(0, 0, 0));
	_restraint->setLinearUpperLimit(BVector(0, 0, 1));
	_restraint->setAngularLowerLimit(BVector(-0.01, -0.01, -0.01));
	_restraint->setAngularUpperLimit(BVector(0.01, 0.01, 0.01));
	_restraint->setName("Sixdof");
}
SixdofHandler::~SixdofHandler()
{

}

void SixdofHandler::attached(BViewer* viewer)
{
	BScene* dscene = viewer->scene();
	dscene->insert(_nodeA);
	dscene->insert(_nodeB);
	dscene->insert(_restraint);
	this->emit("edit-restraint", _restraint);
}
void SixdofHandler::detached(BViewer* viewer)
{
	BScene* dscene = viewer->scene();
	dscene->remove(_nodeA);
	dscene->remove(_nodeB);
	dscene->remove(_restraint);
}

void SixdofHandler::paint(BPainter& painter)
{
	BViewer* viewer = this->viewer();

}
void SixdofHandler::render(BRender& render)
{
	render.setLighting(false);
	render.setDepthTest(false);
	render.setLineSmooth(true);
	render.setLineWidth(1);
	
	if (_restraint)
	{
		BVector origin = _restraint->pivotA();

		const BVector& linearLowerLimit = _restraint->linearLowerLimit();
		const BVector& linearUpperLimit = _restraint->linearUpperLimit();

		BNode* nodeA = _restraint->bodyA();
		BNode* nodeB = _restraint->bodyB();
		BVector pminx = (origin + BVector(linearLowerLimit.x(), 0, 0)) * nodeA->matrix();
		BVector pmaxx = (origin + BVector(linearUpperLimit.x(), 0, 0)) * nodeA->matrix();
		BVector pminy = (origin + BVector(0, linearLowerLimit.y(), 0)) * nodeA->matrix();
		BVector pmaxy = (origin + BVector(0, linearUpperLimit.y(), 0)) * nodeA->matrix();
		BVector pminz = (origin + BVector(0, 0, linearLowerLimit.z())) * nodeA->matrix();
		BVector pmaxz = (origin + BVector(0, 0, linearUpperLimit.z())) * nodeA->matrix();
		origin = origin * _restraint->bodyA()->matrix();
		render.setColor(255, 255, 255);
		render.setLineWidth(1);
		//render.setColor(255, 100, 0);
		render.drawLine(pminx, pmaxx);
		//render.setColor(0, 255, 100);
		render.drawLine(pminy, pmaxy);
		//render.setColor(0, 100, 255);
		render.drawLine(pminz, pmaxz);

		if (BNode* nodeA = _restraint->bodyA())
		{
			BVector pointA = nodeA->matrix() * _restraint->pivotA();
			BVector axis0 = nodeA->matrix().row(0);
			BVector axis1 = nodeA->matrix().row(1);
			BVector axis2 = nodeA->matrix().row(2);
			render.setLineWidth(2);
			render.setColor(255, 100, 0);
			render.drawLine(pointA, pointA + axis0);
			render.setColor(0, 255, 100);
			render.drawLine(pointA, pointA + axis1);
			render.setColor(0, 100, 255);
			render.drawLine(pointA, pointA + axis2);
		}

		if (BNode* nodeB = _restraint->bodyB())
		{
			BVector pointB = nodeB->matrix() * _restraint->pivotB();
			BVector axis0 = nodeB->matrix().row(0);
			BVector axis1 = nodeB->matrix().row(1);
			BVector axis2 = nodeB->matrix().row(2);
			render.setLineWidth(1);
			render.setColor(200, 0, 0);
			render.drawLine(pointB, pointB + axis0);
			render.setColor(0, 200, 0);
			render.drawLine(pointB, pointB + axis1);
			render.setColor(0, 0, 200);
			render.drawLine(pointB, pointB + axis2);
		}
		return;
	}

}

void SixdofHandler::styleEvent(const BEvent& event)
{
	BHandler::styleEvent(event);
	const BStyle* style = event.value();
	BImage* image = style->image("cone-texture");
	if (BShape* shape = _nodeB->shape(0))
	{
		shape->setTexture(image);
	}
}
